#include "Arrive.h"
#include "../Character.h"
#include "../CApp.h"

#include <stdlib.h>
#include <vector>

using namespace std;

Arrive::Arrive(){
  maxAcceleration = 1;
  maxSpeed = 1;
  targetRadius = 40;
  slowRadius = 140;        // Esto es 45 grados.
  timeToTarget = 15;
}

Steering* Arrive::getSteering(){
  // Caminos en el grafo del mapa
  Node *start = NULL, *end = NULL;
  vector<Node *>camino;
  if(theCharacter){
    start = theApp.findNearestNode(character->position);
  }
  if(theTarget){
    end = theApp.findNearestNode(target->position);
  }
  theCharacter->caminoObjetivo.clear();
  if(start && end){
    if(start->getId() != end->getId()){
      return Seek::getSteering();
    }
  }
  // FIN calculo de caminos

  Steering* ret = new Steering();
  float targetSpeed;
  float direction[3];
  float targetVelocity[3];
  restarVectores(target->position, character->position,direction);
  float distance = length(direction);
  if (distance < targetRadius)
    return new Steering();
  else if (distance > slowRadius)
    targetSpeed = maxSpeed;
  else
    targetSpeed = maxSpeed*abs((distance - targetRadius))/ slowRadius;
  targetVelocity[0] = direction[0];
  targetVelocity[1] = direction[1];
  targetVelocity[2] = direction[2];
  normalize(targetVelocity);
  multVectorScalar(targetVelocity,targetSpeed);
  restarVectores(targetVelocity,character->velocity, ret->linear);
  divVectorScalar(ret->linear,timeToTarget);
  if (length(ret->linear) > maxAcceleration){
    normalize(ret->linear);
    multVectorScalar(ret->linear,maxAcceleration);
  }
  return ret;
}
